#include "Vector3D.h"
#include <math.h>

Vector3D::Vector3D(void)
:	x(0), y(0), z(0) {
}

Vector3D::Vector3D(float ix, float iy, float iz)
:	x(ix), y(iy), z(iz) {
}

void Vector3D::setNull() {
	x = y = z = 0;
}

void Vector3D::normalize(void) {
	float invlength = 1 / length();
	x *= invlength;
	y *= invlength;
	z *= invlength;
}

float Vector3D::length() const {
	return sqrt(x * x + y * y + z * z);
}

void Vector3D::add(const Vector3D& v) {
	x += v.x;
	y += v.y;
	z += v.z;
}

bool Vector3D::isEqual(const Vector3D& v) {
	return ((v.x == x) && (v.y == y) && (v.z == z));
}

Vector3D Vector3D::crossProduct(const Vector3D &vec) const {
	return Vector3D(y * vec.z - z * vec.y,
					z * vec.x - x * vec.z,
					x * vec.y - y * vec.x);
}

void Vector3D::operator += (const Vector3D& v) {
	x += v.x;
	y += v.y;
	z += v.z;
}

float Vector3D::dotProduct(const Vector3D& vector) {
    return (x * vector.x + y * vector.y + z * vector.z);
}

// Given a pair of 3D vectors, returns the angle between them.
// Angle is expressed in radians, in range [0, PI]
float Vector3D::angle(const Vector3D& vector) {
    //
    // From dot product definition:
    //
    // a . b = |a| |b| cos(theta)
    //
    //  ==>  cos(theta) = a . b / (|a| |b|)
    //
    //  ==>  theta = acos( a . b / (|a| |b|) )
    //
    return acos( dotProduct(vector) / (length() * vector.length()) );
}

Vector3D operator - (const Vector3D& v) {
	return Vector3D(-v.x, -v.y, -v.z);
}

Vector3D operator + (const Vector3D& v, const Vector3D& w) {
	return Vector3D(v.x + w.x, v.y + w.y, v.z + w.z);
}

Vector3D operator - (const Vector3D& v, const Vector3D& w) {
	return Vector3D(v.x - w.x, v.y - w.y, v.z - w.z);
}

Vector3D operator * (float f, const Vector3D& v) {
	return Vector3D(v.x * f, v.y * f, v.z * f);
}

Vector3D operator * (const Vector3D& v, float f) {
	return Vector3D(v.x * f, v.y * f, v.z * f);
}

float operator * (const Vector3D& v, const Vector3D& w) {
	return v.x * w.x + v.y * w.y + v.z * w.z;
}

float Vector3D::calculateDistance(const Vector3D& v2) {
	return sqrt(pow(x - v2.x, 2) + pow(y - v2.y, 2) + pow(z - v2.z, 2));
}
